/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#include "OptimizableTypes.h"

namespace ORB_SLAM3
{
bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is)
{
    for (int i=0; i<2; i++)
    {
        is >> _measurement[i];
    }
    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            is >> information()(i,j);
            if (i!=j)
                information()(j,i)=information()(i,j);
        }
    return true;
}

bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const
{

    for (int i=0; i<2; i++)
    {
        os << measurement()[i] << " ";
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            os << " " <<  information()(i,j);
        }
    return os.good();
}

/** 
 * @brief 求解二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXi
 */
void EdgeSE3ProjectXYZOnlyPose::linearizeOplus()
{
    g2o::VertexSE3Expmap * vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
    Eigen::Vector3d xyz_trans = vi->estimate().map(Xw);

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];

    Eigen::Matrix<double,3,6> SE3deriv;
    SE3deriv << 0.f, z, -y,   1.f, 0.f, 0.f,
               -z , 0.f, x,   0.f, 1.f, 0.f,
                y , -x , 0.f, 0.f, 0.f, 1.f;

    _jacobianOplusXi = -pCamera->projectJac(xyz_trans) * SE3deriv;
}

bool EdgeSE3ProjectXYZOnlyPoseToBody::read(std::istream& is)
{
    for (int i=0; i<2; i++)
    {
        is >> _measurement[i];
    }
    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            is >> information()(i,j);
            if (i!=j)
                information()(j,i)=information()(i,j);
        }
    return true;
}

bool EdgeSE3ProjectXYZOnlyPoseToBody::write(std::ostream& os) const
{

    for (int i=0; i<2; i++)
    {
        os << measurement()[i] << " ";
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            os << " " <<  information()(i,j);
        }
    return os.good();
}

/** 
 * @brief 求解右目的二维像素坐标关于左目位姿的雅克比矩阵 _jacobianOplusXi
 */
void EdgeSE3ProjectXYZOnlyPoseToBody::linearizeOplus()
{
    // 获得三维点在右相机坐标系下的坐标
    g2o::VertexSE3Expmap * vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
    g2o::SE3Quat T_lw(vi->estimate());
    Eigen::Vector3d X_l = T_lw.map(Xw);
    Eigen::Vector3d X_r = mTrl.map(T_lw.map(Xw));

    double x_w = X_l[0];
    double y_w = X_l[1];
    double z_w = X_l[2];

    Eigen::Matrix<double,3,6> SE3deriv;
    SE3deriv << 0.f,  z_w,  -y_w, 1.f, 0.f, 0.f,
              -z_w ,  0.f,   x_w, 0.f, 1.f, 0.f,
               y_w , -x_w ,  0.f, 0.f, 0.f, 1.f;

    /*
        注意这里是对李代数求导，ρlw != tlw 所以不能使用Pl = Rlw*Pw + tlw
        Pl = EXP(ξlw)*Pw    Pr = Rrl * EXP(ξlw) * Pw + trl  
        让Pr 对 ξlw 求雅克比
        相当于Rrl*(Pl 对 ξlw的雅克比)
     */
    _jacobianOplusXi = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
}

EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>()
{

}

bool EdgeSE3ProjectXYZ::read(std::istream& is)
{
    for (int i=0; i<2; i++)
    {
        is >> _measurement[i];
    }
    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            is >> information()(i,j);
            if (i!=j)
                information()(j,i)=information()(i,j);
        }
    return true;
}

bool EdgeSE3ProjectXYZ::write(std::ostream& os) const
{

    for (int i=0; i<2; i++)
    {
        os << measurement()[i] << " ";
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            os << " " <<  information()(i,j);
        }
    return os.good();
}

/** 
 * @brief 求解二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXj  二维像素坐标关于三维点世界坐标的雅克比矩阵 _jacobianOplusXi  
 */
void EdgeSE3ProjectXYZ::linearizeOplus()
{
    g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
    g2o::SE3Quat T(vj->estimate());
    g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
    Eigen::Vector3d xyz = vi->estimate();
    Eigen::Vector3d xyz_trans = T.map(xyz);

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];

    auto projectJac = -pCamera->projectJac(xyz_trans);
    // Pc = Rcw*Pw + tcw  先求Pw改变对Pc的影响，所以直接为Rcw，前面再乘Pc对像素的影响
    _jacobianOplusXi =  projectJac * T.rotation().toRotationMatrix();  

    Eigen::Matrix<double,3,6> SE3deriv;
    SE3deriv << 0.f, z,  -y,  1.f, 0.f, 0.f,
                -z, 0.f,  x,  0.f, 1.f, 0.f,
                 y, -x, 0.f,  0.f, 0.f, 1.f;

    _jacobianOplusXj = projectJac * SE3deriv;
}

EdgeSE3ProjectXYZToBody::EdgeSE3ProjectXYZToBody() : BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>()
{

}

bool EdgeSE3ProjectXYZToBody::read(std::istream& is)
{
    for (int i=0; i<2; i++)
    {
        is >> _measurement[i];
    }
    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            is >> information()(i,j);
            if (i!=j)
                information()(j,i)=information()(i,j);
        }
    return true;
}

bool EdgeSE3ProjectXYZToBody::write(std::ostream& os) const
{

    for (int i=0; i<2; i++)
    {
        os << measurement()[i] << " ";
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            os << " " <<  information()(i,j);
        }
    return os.good();
}

/** 
 * @brief 求解右目二维像素坐标关于位姿的雅克比矩阵 _jacobianOplusXj  右目二维像素坐标关于三维点世界坐标的雅克比矩阵 _jacobianOplusXi  
 */
void EdgeSE3ProjectXYZToBody::linearizeOplus()
{
    g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
    g2o::SE3Quat T_lw(vj->estimate());
    g2o::SE3Quat T_rw = mTrl * T_lw;
    g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
    Eigen::Vector3d X_w = vi->estimate();
    Eigen::Vector3d X_l = T_lw.map(X_w);
    Eigen::Vector3d X_r = mTrl.map(T_lw.map(X_w));

    _jacobianOplusXi =  -pCamera->projectJac(X_r) * T_rw.rotation().toRotationMatrix();

    double x = X_l[0];
    double y = X_l[1];
    double z = X_l[2];

    Eigen::Matrix<double,3,6> SE3deriv;
    SE3deriv << 0.f, z,  -y,  1.f, 0.f, 0.f,
                -z, 0.f,  x,  0.f, 1.f, 0.f,
                 y, -x, 0.f,  0.f, 0.f, 1.f;
    /*
        注意这里是对李代数求导，ρlw != tlw 所以不能使用Pl = Rlw*Pw + tlw
        Pl = EXP(ξlw)*Pw    Pr = Rrl * EXP(ξlw) * Pw + trl  
        让Pr 对 ξlw 求雅克比
        相当于Rrl*(Pl 对 ξlw的雅克比)
     */
    _jacobianOplusXj = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
}


VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, g2o::Sim3>()
{
    _marginalized = false;
    _fix_scale = false;
}

bool VertexSim3Expmap::read(std::istream& is)
{
    g2o::Vector7d cam2world;
    for (int i=0; i<6; i++)
    {
        is >> cam2world[i];
    }
    is >> cam2world[6];

    float nextParam;
    for(size_t i = 0; i < pCamera1->size(); i++)
    {
        is >> nextParam;
        pCamera1->setParameter(nextParam,i);
    }

    for(size_t i = 0; i < pCamera2->size(); i++)
    {
        is >> nextParam;
        pCamera2->setParameter(nextParam,i);
    }

    setEstimate(g2o::Sim3(cam2world).inverse());
    return true;
}

bool VertexSim3Expmap::write(std::ostream& os) const
{
    g2o::Sim3 cam2world(estimate().inverse());
    g2o::Vector7d lv=cam2world.log();
    for (int i=0; i<7; i++)
    {
        os << lv[i] << " ";
    }

    for(size_t i = 0; i < pCamera1->size(); i++)
    {
        os << pCamera1->getParameter(i) << " ";
    }

    for(size_t i = 0; i < pCamera2->size(); i++)
    {
        os << pCamera2->getParameter(i) << " ";
    }

    return os.good();
}

EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() : g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>()
{
}

bool EdgeSim3ProjectXYZ::read(std::istream& is)
{
    for (int i=0; i<2; i++)
    {
        is >> _measurement[i];
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            is >> information()(i,j);
            if (i!=j)
                information()(j,i)=information()(i,j);
        }
    return true;
}

bool EdgeSim3ProjectXYZ::write(std::ostream& os) const
{
    for (int i=0; i<2; i++)
    {
        os  << _measurement[i] << " ";
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            os << " " <<  information()(i,j);
        }
    return os.good();
}

EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() : g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSim3Expmap>()
{

}

bool EdgeInverseSim3ProjectXYZ::read(std::istream& is)
{
    for (int i=0; i<2; i++)
    {
        is >> _measurement[i];
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            is >> information()(i,j);
            if (i!=j)
                information()(j,i)=information()(i,j);
        }
    return true;
}

bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const
{
    for (int i=0; i<2; i++)
    {
        os  << _measurement[i] << " ";
    }

    for (int i=0; i<2; i++)
        for (int j=i; j<2; j++)
        {
            os << " " <<  information()(i,j);
        }
    return os.good();
}

}
